/*
 * To change this template, choose Tools | Templates
 * and open the template in the editor.
 */
package edu.wpi.first.wpilibj.templates;

import edu.wpi.first.wpilibj.camera.AxisCamera;
import edu.wpi.first.wpilibj.camera.AxisCameraException;
import edu.wpi.first.wpilibj.image.ColorImage;
import edu.wpi.first.wpilibj.image.EllipseDescriptor;
import edu.wpi.first.wpilibj.image.EllipseMatch;
import edu.wpi.first.wpilibj.image.MonoImage;
import edu.wpi.first.wpilibj.image.NIVisionException;
import java.util.Hashtable;

/**
 * The targetting class is resposible for polling
 * the camera for latest image, extracting ellipses
 * and determining the coordinates of target.
 * @author Firebird Robotics
 * @version $Id: $
 */

public class Targetting implements Runnable {

    //calculated info and thread lock
    protected Object lock;
    protected int[] center;
    protected boolean centerDetected;

    //camera and info
    protected AxisCamera camera;
    protected int cameraWidth;
    protected int cameraHeight;
    //ellipse search filters and results
    protected EllipseDescriptor ellipseDescriptor;
    protected EllipseMatch[] targets;
    //string to string properties reference
    protected Hashtable properties;


    //property names
    public static final String PROPERTY_MIN_MAJOR = "ellipse.min.major";
    public static final String PROPERTY_MAX_MAJOR = "ellipse.max.major";
    public static final String PROPERTY_MIN_MINOR = "ellipse.min.minor";
    public static final String PROPERTY_MAX_MINOR = "ellipse.max.minor";
    public static final String PROPERTY_CENTER_DELTA = "ellipse.center.delta";
    public static final String PROPERTY_SLEEP_PERIOD = "sleep.period.millis";


    //default values, if not specified in properties
    public static final double DEFAULT_MIN_MAJOR = 20.0;
    public static final double DEFAULT_MAX_MAJOR = 100.0;
    public static final double DEFAULT_MIN_MINOR = 20.0;
    public static final double DEFAULT_MAX_MINOR = 100.0;
    public static final double DEFAULT_CENTER_DELTA = 10;
    public static final int DEFAULT_SLEEP_PERIOD = 0;
    protected double minMajor, maxMajor, minMinor, maxMinor, delta;
    protected int timeout;

    //-------------------------------------------------------------------
    /**
     * Constructor with no property overrides
     */
    public Targetting(AxisCamera camera) {
        this(camera, null);
    }

    //-------------------------------------------------------------------
    /**
     * Constructor
     * @param camera Instance of robot camera
     * @param properties Settings for this instance, can be null
     */
    public Targetting(AxisCamera camera, Hashtable properties) {
        this.camera = camera;
        this.properties = properties;

        init();
    }

    //-------------------------------------------------------------------
    /**
     * Initializes this instance
     */
    protected void init() {
        //this is used to synchronize threads
        this.lock = new Object();

        //image settings
        this.cameraWidth = this.camera.getResolution().width;
        this.cameraHeight = this.camera.getResolution().height;

        //assign defaults
        this.minMajor = DEFAULT_MIN_MAJOR;
        this.maxMajor = DEFAULT_MAX_MAJOR;
        this.minMinor = DEFAULT_MIN_MINOR;
        this.maxMinor = DEFAULT_MAX_MINOR;
        this.delta    = DEFAULT_CENTER_DELTA;
        this.timeout  = DEFAULT_SLEEP_PERIOD;

        //check for overrides
        if (this.properties != null) {
            readProperties();
        }

        //create the ellipse descriptor
        this.ellipseDescriptor = new EllipseDescriptor(this.minMajor, this.maxMajor,
                this.minMinor, this.maxMinor);
    }

    //-------------------------------------------------------------------
    /**
     * Examines properties for overrides. Since properties are in string type,
     * the values must be parsed according to intended type.  Parse issues,
     * usually NumberFormatExceptions, are caught and passed to the caller
     * IllegalArgumentException's.
     * @throws IllegalArgumentException If parse error occurs
     */
    protected void readProperties() throws IllegalArgumentException {
        if (properties == null || properties.isEmpty()) {
            return;
        }

        double dTemp;
        int iTemp;
        String strVal = null;

        strVal = this.getProperty(PROPERTY_MAX_MINOR);
        if (strVal != null) {
            try {
                dTemp = Double.parseDouble(strVal);
                this.maxMinor = dTemp;
            } catch (NumberFormatException nfEx) {
                throw new IllegalArgumentException("Could not parse value for " +
                        PROPERTY_MAX_MINOR + ": " + strVal);
            }

        }

        strVal = this.getProperty(PROPERTY_MIN_MINOR);
        if (strVal != null) {
            try {
                dTemp = Double.parseDouble(strVal);
                this.minMinor = dTemp;
            } catch (NumberFormatException nfEx) {
                throw new IllegalArgumentException("Could not parse value for " +
                        PROPERTY_MIN_MINOR + ": " + strVal);
            }
        }

        strVal = this.getProperty(PROPERTY_MAX_MAJOR);
        if (strVal != null) {
            try {
                dTemp = Double.parseDouble(strVal);
                this.maxMajor = dTemp;
            } catch (NumberFormatException nfEx) {
                throw new IllegalArgumentException("Could not parse value for " +
                        PROPERTY_MAX_MAJOR + ": " + strVal);
            }
        }

        strVal = this.getProperty(PROPERTY_MIN_MAJOR);
        if (strVal != null) {
            try {
                dTemp = Double.parseDouble(strVal);
                this.minMajor = dTemp;
            } catch (NumberFormatException nfEx) {
                throw new IllegalArgumentException("Could not parse value for " +
                        PROPERTY_MIN_MAJOR + ": " + strVal);
            }
        }

        strVal = this.getProperty(PROPERTY_CENTER_DELTA);
        if (strVal != null) {
            try {
                dTemp = Double.parseDouble(strVal);
                this.delta = dTemp;
            } catch (NumberFormatException nfEx) {
                throw new IllegalArgumentException("Could not parse value for " +
                        PROPERTY_CENTER_DELTA + ": " + strVal);
            }
        }

        strVal = this.getProperty(PROPERTY_SLEEP_PERIOD);
        if (strVal != null) {
            try {
                iTemp = Integer.parseInt(strVal);
                this.timeout = iTemp;
            } catch (NumberFormatException nfEx) {
                throw new IllegalArgumentException("Could not parse value for " +
                        PROPERTY_SLEEP_PERIOD + ": " + strVal);
            }

        }
    }


    //-------------------------------------------------------------------

    protected String getProperty(String propertyName)
    {
        String value = null;

        if (this.properties != null)
        {
            if (this.properties.containsKey(propertyName))
            {
                value = this.properties.get(propertyName).toString();
            }
        }

        return value;

    }

    //-------------------------------------------------------------------
    /**
     * Cycles through collecting statistics from image and calculating
     * estimate center in image.
     */
    public void run() {

        //TODO - make this while(flag) so that it turns off
        while (true)
        {
            try {
                pollImage();
                calculateCenter();
                nap();
            } catch (NIVisionException nivEx) {
                nivEx.printStackTrace();
            } catch (Exception ex) {
                ex.printStackTrace();
            }
        }
    }

    //-------------------------------------------------------------------
    protected void pollImage() throws AxisCameraException, NIVisionException {
        //not really necessary, but in case targets becomes something
        //with memory we have to manage, putting line here
        if (this.targets != null) {
            this.targets = null;
        }

        //get the color image, then its intensity, perform ell detection on that
        ColorImage tmpColor = this.camera.getImage();
        if (tmpColor != null) {
            MonoImage tmpMono = tmpColor.getLuminancePlane();

            if (tmpMono != null) {
                this.targets = tmpMono.detectEllipses(this.ellipseDescriptor);

                tmpMono.free();
            }
            tmpColor.free();
        }

    }

    //-------------------------------------------------------------------
    /**
     * If a timeout was specified, sleep for that period.  If zero timeout,
     * this method will return immediately without pause
     */
    protected void nap() {
        if (this.timeout > 0) {
            try {
                Thread.sleep(this.timeout);
            } catch (InterruptedException iEx) {
                //rarely happens
                iEx.printStackTrace();
            }
        }
    }

    //-------------------------------------------------------------------
    /**
     * Returns an integer array representing the (x,y) coordinate of the
     * detected center of target, in pixels.  If no target is detected,
     * an array of length 0 is returned.
     * @return Target center coordinates, or zero-length array if not found
     */
    public int[] getCenterImageCoordinates() {

        int[] newVal = new int[0];

        synchronized (lock) {
            if (this.centerDetected) {
                newVal = new int[]{center[0], center[1]};
            }
        }

        return newVal;
    }

    //-------------------------------------------------------------------
    /**
     * Returns a double array representing the (x,y) coordinate of the
     * range of the image, between [-1,1] for each axis, of the detected
     * center of target.
     * The center of the image is (0,0).
     * If no target is detected, an array of length 0 is returned.
     * @return Target center coordinates, or zero-length array if not found
     */
    public double[] getCenterImageRange() {

        double[] newVal = new double[0];

        synchronized (lock) {
            if (this.centerDetected) {
                newVal = new double[2];
                double halfWidth = this.cameraWidth / 2.0;
                double halfHeight = this.cameraHeight / 2.0;
                newVal[0] = ((double) center[0] - halfWidth) / halfWidth;
                newVal[1] = ((double) center[1] - halfHeight) / halfHeight;
            }
        }
        return newVal;
    }

    //-------------------------------------------------------------------
    protected void calculateCenter() throws NIVisionException {
        System.out.println(targets.length + " : Number of targets");

        synchronized (lock) {

            //TODO For now, we assume the two biggest have similar center.
            // It would be better to search the first N, and select a pair
            // close centers AND with ratios similar to that of the two
            // ellipses in the target.
            if (targets.length < 2) {
                this.centerDetected = false;
            } else {
                EllipseMatch first = targets[0];
                EllipseMatch second = targets[1];
                if ((Math.abs(first.m_xPos - second.m_xPos) < delta) &&
                        (Math.abs(first.m_yPos - second.m_yPos) < delta)) {
                    this.center[0] = (int) (first.m_xPos + second.m_xPos + .5) / 2;
                    this.center[1] = (int) (first.m_yPos + second.m_yPos + .5) / 2;
                    this.centerDetected = true;
                } else {
                    this.centerDetected = false;
                }
            }
        } //end_sync_block
    }
}
